#pragma once
#include "CoordSys.h"
#include "GNSSSatPos.h"
#include "ReadO.h"
#include "Matrix.h"
#include <vector>
#include <stdexcept>

class PointPosition {
public:
    PointPosition(CoordSys* coordSys, TimeSys* timeSys,
        ReadN* readN, ReadO* readO);

    BLH calculatePosition(int maxIter = 10, double convThreshold = 0.01);
    void test();

private:
    void buildEquationSystem(const XYZ& approxPos, const obsEpoch& obs,
        Matrix& designMatrix, Matrix& obsVector);

    bool solveLeastSquare(const Matrix& A, const Matrix& L,
        Matrix& solution);
    double matrixNorm(const Matrix& m) const;

    CoordSys* mCoordSys;
    TimeSys* mTimeSys;
    ReadN* mReadN;
    ReadO* mReadO;
};